﻿/*****************************************************************//**
 * \file   SurgicalHapticControlRobotThread.cpp
 * \brief  haptic 控制机械臂线程类
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/
#include "./include/SurgicalHapticControlRobotThread.h"
#include "./include/SurgicalRobotCommon.h"
#include <semaphore.h>
#include <QDebug>
#include<iostream>





 /**
  * \brief   SurgicalHapticControlRobotThread类构造函数.
  *
  */
SurgicalHapticControlRobotThread::SurgicalHapticControlRobotThread()
{
	bControlStart = false;
	m4dHapticStartposition[0].setZero();
	m4dHapticNewposition[0].setZero();
	m4dToolNewposition[0].setZero();
	m4dToolStartposition[0].setZero();
	m3dRotationSimga2robot.setZero();

#if DEBUG_HAPTIC_CONTROL_ROBOT
	time_experimrnt.open("C:\\feng\\experiment_result\\experiment_feng\\vf_contraint\\time_3\\time1.txt", std::ios::out);
#endif 
}

/**
 * \brief   SurgicalHapticControlRobotThread析构函数.
 *
 */
SurgicalHapticControlRobotThread::~SurgicalHapticControlRobotThread()
{

}

/**
 * \brief   槽函数.
 *
 */
void SurgicalHapticControlRobotThread::HapticControlRobotStart()
{
	//主手初始位姿矩阵————补偿重力补偿模式下的主手位置与姿态的偏移
	m3dRotationSimga2robot << cos(M_PI / 4), -sin(M_PI / 4), 0, sin(M_PI / 4), cos(M_PI / 4), 0, 0, 0, 1;
	surgicalRobotCom->HDControl[0].GetHapticPosture(sHapticRunData[0]);
	m4dHapticStartposition[0] = sHapticRunData[0].dhd_PostureMatrix;
	m4dHapticNewposition[0] = sHapticRunData[0].dhd_PostureMatrix;
	m4dToolStartposition[0] = sRobotRunData[0].sMotorDevice.m4dToolEnd;
	bControlStart = true;
}

/**
 * \brief   槽函数.
 *
 */
void SurgicalHapticControlRobotThread::HapticControlRobotStop()
{
	bControlStart = false;
}

/**
 * \brief   Haptic 控制机械臂线程函数.
 *
 */
void SetThreadAffinity(int cpu)
{
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(cpu, &cpuset);

    pthread_t currentThread = pthread_self();  // 获取当前线程

    int result = pthread_setaffinity_np(currentThread, sizeof(cpu_set_t), &cpuset);
    if (result != 0) {
        // 处理错误
    }
}

void SurgicalHapticControlRobotThread::run_code()
{
    Eigen::Matrix4d m4dNext;
    Eigen::Matrix4d m4dToolEndNext;
    Eigen::Vector3d v3dRCMTemp;
    Eigen::Matrix3d m3dOri;
    Eigen::Vector3d v3dTransposition;

    SetThreadAffinity(3);  // 将线程绑定到 CPU 3

    sem_init(&hHapticControlRobotSemaphore, 0, 0);

    while (true)
    {
        iControl++;

#if DEBUG_HAPTIC_CONTROL_ROBOT
        qDebug() << iSurgical;
#endif

        if (sem_wait(&hHapticControlRobotSemaphore) == -1) {
            if (errno == EINTR) {
                // 处理信号中断
            } else {
                // 处理其他错误
            }
        }

        if (bControlStart) {
            m4dHapticNewposition[0] = sHapticRunData[0].dhd_PostureMatrix;

            if ((m4dHapticNewposition[0].block(0, 3, 3, 1) - m4dHapticStartposition[0].block(0, 3, 3, 1)).norm() == 0) {
                m4dToolNewposition[0].block(0, 3, 3, 1) = (m4dToolStartposition[0].block(0, 3, 3, 1) + m3dRotationSimga2robot * (m4dHapticNewposition[0].block(0, 3, 3, 1) - m4dHapticStartposition[0].block(0, 3, 3, 1)));
            } else {
                v3dSimgaMove = (m4dHapticNewposition[0].block(0, 3, 3, 1) - m4dHapticStartposition[0].block(0, 3, 3, 1));
                dScaleFactor = 0.8;
                v3dSimgaMove(0) = dScaleFactor * v3dSimgaMove(0);
                v3dSimgaMove(1) = dScaleFactor * v3dSimgaMove(1);
                dScaleFactor = 0.9;
                v3dSimgaMove(2) = dScaleFactor * v3dSimgaMove(2);

                m4dToolNewposition[0].block(0, 3, 3, 1) = (m4dToolStartposition[0].block(0, 3, 3, 1) + m3dRotationSimga2robot * v3dSimgaMove);
            }

            m4dToolNewposition[0].block(0, 0, 3, 3) = m4dToolStartposition[0].block(0, 0, 3, 3) * (m4dHapticStartposition[0].block(0, 0, 3, 3).inverse() * m4dHapticNewposition[0].block(0, 0, 3, 3));

            m3dOri = m4dToolNewposition[0].block(0, 0, 3, 3);
            v3dTransposition = m4dToolNewposition[0].block(0, 3, 3, 1);

            m4dToolEndNext.block(0, 0, 3, 3) = m3dOri;
            m4dToolEndNext.block(0, 3, 3, 1) = v3dTransposition;
            m4dToolEndNext.block(3, 0, 1, 4) << 0.0, 0.0, 0.0, 1.0;

            v3dRCMTemp(0) = sRobotConfigPara[RobotArm0].dRCMPosition[0];
            v3dRCMTemp(1) = sRobotConfigPara[RobotArm0].dRCMPosition[1];
            v3dRCMTemp(2) = sRobotConfigPara[RobotArm0].dRCMPosition[2];

            m4dNext = surgicalRobotCom->endTool[RobotArm0]->getRobotNextEnd(sRobotRunData[RobotArm0].sRobotDy.m4dRobotEnd, m4dToolEndNext, v3dRCMTemp);

            if (surgicalRobotCom->robotControl[RobotArm0] && (surgicalRobotCom->robotControl[RobotArm0]->bInitDone)) {
                surgicalRobotCom->robotControl[RobotArm0]->robotMove(m4dNext);
            }
        }
        else {
            qDebug() << "Waitting for pushing the  haptic control robot button";
            usleep(1000 * 1000);  // 1 秒延迟
        }
    }
}
//void SurgicalHapticControlRobotThread::run()
//{
//	Eigen::Matrix4d m4dNext;
//	//机械臂末端位置与姿态矩阵-HapticControlRobot
//	Eigen::Matrix4d m4dToolEndNext;
//	Eigen::Vector3d v3dRCMTemp;
//	Eigen::Matrix3d m3dOri;
//	Eigen::Vector3d v3dTransposition;
//	SetThreadAffinityMask(GetCurrentThread(), 8);
//	while (true)
//	{
//		iControl++;
//		//qDebug() << "iControl:" << iControl;
//		//qDebug() << "iGet:" << iGet;
//		//qDebug() << "iCamera:" << iCamera;
//#if DEBUG_HAPTIC_CONTROL_ROBOT
//		qDebug() << iSurgical;
//#endif
//		WaitForSingleObject(hHapticControlRobotSemaphore, INFINITE);

//		if (bControlStart) {
//			//实际运行时需要去掉，比较耗时。
//			//qDebug() << "SurgicalHapticControlRobotThread runing";
//			m4dHapticNewposition[0] = sHapticRunData[0].dhd_PostureMatrix;

//			// 主手的平移运动缩放,scaleFactor即为缩放比例
//			if ((m4dHapticNewposition[0].block(0, 3, 3, 1) - m4dHapticStartposition[0].block(0, 3, 3, 1)).norm() == 0) {
//#if DEBUG_HAPTIC_CONTROL_ROBOT
//				//time_experimrnt << "1" << "\t" << m4dHapticNewposition[0].block(0, 3, 1, 1) << "\t" << m4dHapticNewposition[0].block(1, 3, 1, 1) << "\t" << m4dHapticNewposition[0].block(2, 3, 1, 1) << "\t" << std::endl;
//#endif
//				m4dToolNewposition[0].block(0, 3, 3, 1) = (m4dToolStartposition[0].block(0, 3, 3, 1) + m3dRotationSimga2robot * (m4dHapticNewposition[0].block(0, 3, 3, 1) - m4dHapticStartposition[0].block(0, 3, 3, 1)));
//			}
//			else {
//				v3dSimgaMove = (m4dHapticNewposition[0].block(0, 3, 3, 1) - m4dHapticStartposition[0].block(0, 3, 3, 1));
//				dScaleFactor = 0.8;
//				v3dSimgaMove(0) = dScaleFactor * v3dSimgaMove(0);
//				v3dSimgaMove(1) = dScaleFactor * v3dSimgaMove(1);
//				dScaleFactor = 0.9;
//				v3dSimgaMove(2) = dScaleFactor * v3dSimgaMove(2);
//#if DEBUG_HAPTIC_CONTROL_ROBOT
//				//time_experimrnt << "0" << "\t" << m4dHapticNewposition[0].block(0, 3, 1, 1) << "\t" << m4dHapticNewposition[0].block(1, 3, 1, 1) << "\t" << m4dHapticNewposition[0].block(2, 3, 1, 1) << "\t" << std::endl;
//#endif
//				m4dToolNewposition[0].block(0, 3, 3, 1) = (m4dToolStartposition[0].block(0, 3, 3, 1) + m3dRotationSimga2robot * v3dSimgaMove);
//			}

//			//通过在主从操控循环之前记录机械臂末端和simga.7的位姿，来补偿重力补偿模式下的主手位置与姿态的偏移
//			//tool_newposition[0].block(0, 3, 3, 1) = (tool_startposition[0].block(0, 3, 3, 1) + rotation_simga2robot * (haptic_newposition[0].block(0, 3, 3, 1) - haptic_startposition[0].block(0, 3, 3, 1)));
//			m4dToolNewposition[0].block(0, 0, 3, 3) = m4dToolStartposition[0].block(0, 0, 3, 3) * (m4dHapticStartposition[0].block(0, 0, 3, 3).inverse() * m4dHapticNewposition[0].block(0, 0, 3, 3));

//			m3dOri = m4dToolNewposition[0].block(0, 0, 3, 3);
//			v3dTransposition = m4dToolNewposition[0].block(0, 3, 3, 1);

//			m4dToolEndNext.block(0, 0, 3, 3) = m3dOri;
//			m4dToolEndNext.block(0, 3, 3, 1) = v3dTransposition;
//			m4dToolEndNext.block(3, 0, 1, 4) << 0.0, 0.0, 0.0, 1.0;

//			//RCM 点设置在某一个特定地方的器械的一个点处
//			v3dRCMTemp(0) = sRobotConfigPara[RobotArm0].dRCMPosition[0];
//			v3dRCMTemp(1) = sRobotConfigPara[RobotArm0].dRCMPosition[1];
//			v3dRCMTemp(2) = sRobotConfigPara[RobotArm0].dRCMPosition[2];

//			m4dNext = surgicalRobotCom->endTool[RobotArm0]->getRobotNextEnd(sRobotRunData[RobotArm0].sRobotDy.m4dRobotEnd, m4dToolEndNext, v3dRCMTemp);

//			if (surgicalRobotCom->robotControl[RobotArm0] && (surgicalRobotCom->robotControl[RobotArm0]->bInitDone)) {
//				//surgicalRobotCom->robotControl[RobotArm0]->robotMoveJ(m4dNext);
//				surgicalRobotCom->robotControl[RobotArm0]->robotMove(m4dNext);
//			}


//		}
//		else {
//			qDebug() << "Waitting for pushing the  haptic control robot button";
//			msleep(1000);
//		}
//	}
//}
